﻿#include "robothandkuka.h"

#include "RobotHand_log.h"
#include "../commontools/commonFunctionSet.h"

constexpr int g_dataLength = 64;
constexpr int g_stateLength = 80;
RobotHandKuka::RobotHandKuka(QObject* parent)
	: RobotHandCommunication(parent)
{
	m_brand = "KUKA";
	log_info("-----------------Kuka Robot has been create-------------");
	sendMutex = new QMutex();
	m_state = new TcpSocket();
	connect(m_state, &TcpSocket::DataReady,
		this, &RobotHandKuka::OnStatusDataReady);
	connect(m_state, &TcpSocket::Disconnected,
		this, &RobotHandKuka::OnStatusConnectionLost);
	m_cmds = new TcpSocket();
	connect(m_cmds, &TcpSocket::DataReady,
		this, &RobotHandKuka::OnCommandDataReady);
	connect(m_cmds, &TcpSocket::Disconnected,
		this, &RobotHandKuka::OnCommandConnectionLost);
}

RobotHandKuka::~RobotHandKuka()
{}

// 初始
int RobotHandKuka::Init(const RobotParam& param)
{
	// 参数校验
	if (param.id.isEmpty() || param.ip.isEmpty()) {
		log_error("Kuka robot init param error, id is [{}] or ip is [{}]", param.id, param.ip);
		return -1;
	}
	if (param.statusPort <= 0 || param.cmdPort <= 0) {
		log_error("Kuka robot init prot error, state is [{}] or cmd is [{}]", param.statusPort, param.cmdPort);
		return -1;
	}
	m_info = param;
	if (!m_state->ConnectToServer(m_info.ip, m_info.statusPort)) {
		log_error("state port connect failed");
		emit ConnectionStateChanged(-3);
		return -3;
	}

	if (!m_cmds->ConnectToServer(m_info.ip, m_info.cmdPort)) {
		log_error("cmd port connect failed");
		emit ConnectionStateChanged(-2);
		return -2;
	}
	emit ConnectionStateChanged(0);	// 连接成功
	m_lastStateData.clear();
	return 0;
}



/****************************************************************************
* 功能: 重连接口
* 传参:
* 返回值: 重连是否成功返回0， 状态端口失败返回-1，命令端口失败返回-2, 全部失败返回-3
****************************************************************************/
int RobotHandKuka::Reconnecte()
{
	if (!m_state->ConnectToServer(m_info.ip, m_info.statusPort)) {
		log_error("state port connect failed");
		emit ConnectionStateChanged(-3);
		return -3;
	}

	if (!m_cmds->ConnectToServer(m_info.ip, m_info.cmdPort)) {
		log_error("cmd port connect failed");
		emit ConnectionStateChanged(-2);
		return -2;
	}
	emit ConnectionStateChanged(0);	// 连接成功
	return 0;
}
int RobotHandKuka::GetConnectedState()
{
	return Reconnecte();
}
/****************************************************************************
* 功能: 停止接口，机械手停止运动并清理运动轨迹
* 传参:
* 返回值:
****************************************************************************/
bool RobotHandKuka::Stop()
{
	log_trace("Robot stop cmd");
	if (SendCmdData(1, "")) {
		return true;
	} else {
		log_error("robot set Stop failed");
		return false;
	}
}

/****************************************************************************
	* 功能: 暂停/恢复接口，机械手暂停，不清理轨迹，机械手恢复后继续运动
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandKuka::Pause()
{
	log_trace("robot set pause");
	QByteArray data = CommonFun::ToBin(2, 4);
	if (SendCmdData(2, data)) {
		return true;
	} else {
		log_error("robot set pause failed");
		return false;
	}
}
bool RobotHandKuka::Resume()
{
	log_trace("robot set pause");
	QByteArray data = CommonFun::ToBin(1, 4);
	if (SendCmdData(2, data)) {
		return true;
	} else {
		log_error("robot set pause failed");
		return false;
	}
}

/****************************************************************************
	* 功能: 设置和获取当前的TCPBase(未启用)
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandKuka::SetTcpBase(const QString& info)
{
	return false;
}
QString RobotHandKuka::GetTcpBase()
{
	return "";
}

/****************************************************************************
	* 功能: 设置DO
	* 传参: index: DO编号(1-n), value: true=1,false=0;
	* 返回值:
	****************************************************************************/
bool RobotHandKuka::SetDo(unsigned int index, bool value)
{
	QByteArray data = CommonFun::ToBin(1, 4);	// index
	data.append(CommonFun::ToBin((int)index, 4));		// val;
	data.append(CommonFun::ToBin(value ? 1 : 0, 4));	// bits;
	log_trace("Set DO index [{}] to [{}]", index, value ? 1 : 0);
	if (SendCmdData(4, data)) {
		return true;
	} else {
		log_error("robot set DO failed");
		return false;
	}
}

/****************************************************************************
	* 功能: 夹具动作/预定义动作
	* 传参: index: 预先定义的动作编号（由机械手内部程序定义）
	* 返回值:
	****************************************************************************/
bool RobotHandKuka::SetGrip(unsigned int index)
{
	QByteArray data = CommonFun::ToBin((int)index, 4);	// index
	log_trace("Set grip index [{}] to [{}]", index);
	if (SendCmdData(5, data)) {
		return true;
	} else {
		log_error("robot set grip failed");
		return false;
	}
}

/****************************************************************************
	* 功能: 移动指令
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandKuka::MoveJL(const QList<MoveJLInfo>& moves)
{
	log_trace("movejl size = {}", moves.size());
	for (auto& iter : moves) {
		QByteArray data = CommonFun::ToBin(1, 4);
		data.append(CommonFun::ToBin(iter.pointType, 4));	// 点的类型
		data.append(CommonFun::ToBin(iter.id, 4));		// 点 id
		data.append(CommonFun::ToBin(iter.pos.x, 4));		// 坐标
		data.append(CommonFun::ToBin(iter.pos.y, 4));
		data.append(CommonFun::ToBin(iter.pos.z, 4));
		data.append(CommonFun::ToBin(iter.pos.a, 4));
		data.append(CommonFun::ToBin(iter.pos.b, 4));
		data.append(CommonFun::ToBin(iter.pos.c, 4));
		data.append(CommonFun::ToBin(iter.vel, 4));		// 速度
		data.append(CommonFun::ToBin(iter.zone, 4));	// 圆滑
		data.append(CommonFun::ToBin(iter.acc, 4));		// 加速度
		data.append(CommonFun::ToBin(iter.di, 4));		// 停止di
		if (!SendCmdData(6, data)) {
			log_error(" Send movejl info error,xyz:({}, {}, {}),abc：({}, {}, {})", iter.pos.x, iter.pos.y, iter.pos.z,
				iter.pos.a, iter.pos.b, iter.pos.c);
			return false;
		}
	}
	return true;
}

/****************************************************************************
	* 功能: 特殊指令
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandKuka::ExecCmd(const QString& cmd, const QString& para, QString& result)
{
	return false;
}

void RobotHandKuka::ParaseRobotStateData()
{
	int index = 0;
	QVector<int> length = { 4 * 6, 4 * 6, 4, 4, 4, 4, 4, 4, 4, 4 };
	QByteArray data = m_lastStateData;
	QString DI, DO;
	//log_error("data:{}", data.toHex());
	while (data.length() >= length.at(index)) {
		QByteArray temp = data.left(length.at(index));
		data.remove(0, length.at(index));
		switch (index) {
		case 0: // 关节角
		{
			auto d = ParseJoints(temp);
			emit RobotHandJoints(QString(d));

		}
		break;
		case 1: // 坐标
		{
			auto d = ParseTargets(temp);
			emit RobotHandTargets(QString(d));
		}
		break;
		case 2:	// 运动点ID
		{
			int id = ParsePointId(temp);
			if (id > 0) {
				log_trace("point id = {}", id);
				emit RobotHandPointId(id);
			}
		}
		break;
		case 3:	// 移动状态
		{
			int id = ParseMoving(temp);
			emit RobotHandMoving(id);
		}
		break;
		case 4:	// 夹具编号
			emit RobotHandToolID(ParseToolId(temp));
			break;
		case 5:// 错误码
		{
			int fault = ParseFaultCode(temp);
			if (fault > 0) {
				//log_error("robot have fault = {}", fault);
				QString msg = GetFaultMsg(fault);
				emit RobotHandFaultCode(fault, msg);
			}
		}
		break;
		case 6:	// di
			DI.clear();
			DI = ParseGi16(temp.mid(0,2));
			//log_error("DI:{}, temp{}", DI, temp.mid(0, 2).toHex());
			break;
		case 7:	// do
			DO.clear();
			DO = ParseGo16(temp.mid(0, 2));
			break;
		case 8:
			DI.append(ParseGi16Extend(temp.mid(0, 2)));
			//log_error("DI:{}, temp{}", DI, temp.mid(0, 2).toHex());
			emit RobotHandDIInfo(DI);
			break;
		case 9:
			DO.append(ParseGo16Extend(temp.mid(0, 2)));
			emit RobotHandDOInfo(DO);
			break;
		default:
			break;
		}

		index = (index + 1) % length.size();
	}
}


// 状态端口消息读取
void RobotHandKuka::OnStatusDataReady(const QByteArray& data)
{
	// 状态数据解析
	//qDebug() << data.toHex();
	//log_error("{}", data.toHex());
	for (int i = 0; i * g_stateLength < data.size(); i++) {
		auto info = data.mid(i * g_stateLength, g_stateLength);
		if (info.size() != g_stateLength) {
			log_error("status size error ={}", info.toHex());
			continue;
		}
		if (info == m_lastStateData && m_stateTimes < 100) {	// 数据未改变且未超过最大此时，不需要解析数据
			m_stateTimes++;
			continue;
		}
		m_stateTimes = 0;
		m_lastStateData = info;
		ParaseRobotStateData();	// 解析状态数据
	}
}
void RobotHandKuka::OnStatusConnectionLost()
{
	log_error("state port {} is disconnected", m_info.statusPort);
	emit ConnectionStateChanged(-1);
}

// 状态端口消息读取
void RobotHandKuka::OnCommandDataReady(const QByteArray& data)
{
	QMutexLocker locker(&cmdDataMutex);
	cmdRecvData.append(data);
}
void RobotHandKuka::OnCommandConnectionLost()
{
	log_error("command port {} is disconnected", m_info.cmdPort);
	emit ConnectionStateChanged(-2);
}

bool RobotHandKuka::SendCmdData(int type, const QByteArray& data)
{
	QMutexLocker locker(sendMutex);
	m_msgIndex++;
	{
		QMutexLocker locker(&cmdDataMutex);
		cmdRecvData.clear();
	}
	
	QByteArray send = CommonFun::ToBin(g_dataLength, 4);	// 消息长度
	send.append(CommonFun::ToBin((int)m_msgIndex, 4)); // 消息编号
	send.append(CommonFun::ToBin(type, 4));	// 命令类型
	send.append(data);		// 命令数据
	while (send.size() < g_dataLength) {	// 消息填充
		send.append('\0');
	}
	log_trace("send to rob: {}", send.toHex());
	if (!m_cmds->WriteData(send)) {
		log_error("send error");
		return false;
	}
	return WaitForCmdAns(m_msgIndex, type);
}

bool RobotHandKuka::WaitForCmdAns(int index, int type)
{
	int times = 200;
	do {
		QByteArray ans;
		{
			QMutexLocker locker(&cmdDataMutex);
			if (!cmdRecvData.isEmpty()) {
				ans = cmdRecvData.takeFirst();
			}
		}
		if (!ans.isEmpty()) {
			int  ret = CheckCmdReturnVal(index, type, ans);
			if (ret == 0) {
				return true;
			} else if (ret < 0) {
				log_error("msg ans error");
				return false;
			}
		}
		CommonFun::Sleep(15);
	} while (times--);
	log_error("WaitForCmdAns time out");
	emit ConnectionStateChanged(-2);
	return false;
}

/****************************************************************************
 * 功能: 校验命令返回值是否正确
 * 传参: 命令编号：index, 命令类型: type, 返回消息：ans
 * 返回值: 0: 消息校验正常， 1：还需要继续等待回复，-1：消息校验失败，无法收到回复
 ****************************************************************************/
int RobotHandKuka::CheckCmdReturnVal(int index, int type, const QByteArray& ans)
{
	log_trace("check ans for type[{}], ans : {}", type, ans.toHex());
	if (ans.size() < g_dataLength) {	// 消息长度异常无法判断，继续等待
		log_error("ans size error");
		return 1;
	}

	int idx = CommonFun::FromBinToInt(ans.mid(0, 4));
	if (index != idx) {
		log_error("ans index: {}, cmd index:{}", idx, index);
		return -1;	// 消息编号不对应，继续判断下一条消息
	}
	int cmd = CommonFun::FromBinToInt(ans.mid(4, 4));
	if (cmd != type) {
		log_error("ans cmd type: {}, cmd type:{}", cmd, type);
		return -1;	// 编号对应，但是回复的内容不对，直接返回错误
	}
	return 0;
}
QString RobotHandKuka::GetFaultMsg(int fault)
{
	switch (fault) {
	case 0:
		return {};
	case 1:
		return "通用错误：检查是否为手动模式";
	default:
		return "位置错误";
		break;
	}
	return {};
}


QByteArray RobotHandKuka::ParseJoints(const QByteArray& d)
{
	Joints j = {};
	j.joint1 = CommonFun::FromBinToFloat(d.mid(0, 4));
	j.joint2 = CommonFun::FromBinToFloat(d.mid(4, 4));
	j.joint3 = CommonFun::FromBinToFloat(d.mid(8, 4));
	j.joint4 = CommonFun::FromBinToFloat(d.mid(12, 4));
	j.joint5 = CommonFun::FromBinToFloat(d.mid(16, 4));
	j.joint6 = CommonFun::FromBinToFloat(d.mid(20, 4));

	return j.toByteArray();
}

QByteArray RobotHandKuka::ParseTargets(const QByteArray& d)
{
	PositionJ m_pos = {};
	m_pos.x = CommonFun::FromBinToFloat(d.mid(0, 4));
	m_pos.y = CommonFun::FromBinToFloat(d.mid(4, 4));
	m_pos.z = CommonFun::FromBinToFloat(d.mid(8, 4));
	m_pos.a = CommonFun::FromBinToFloat(d.mid(12, 4));
	m_pos.b = CommonFun::FromBinToFloat(d.mid(16, 4));
	m_pos.c = CommonFun::FromBinToFloat(d.mid(20, 4));
	return m_pos.toByteArray();
}
int RobotHandKuka::ParsePointId(const QByteArray& d)
{
	return CommonFun::FromBinToInt(d);
}
int RobotHandKuka::ParseMoving(const QByteArray& d)
{
	//qDebug() << d.toHex();
	return CommonFun::FromBinToInt(d);
}
int RobotHandKuka::ParseToolId(const QByteArray& d)
{
	//qDebug() << d.toHex();
	return CommonFun::FromBinToInt(d);
}
int RobotHandKuka::ParseFaultCode(const QByteArray& d)
{
	//qDebug() << d.toHex();
	return CommonFun::FromBinToInt(d);
}
QString RobotHandKuka::ParseGi16(const QByteArray& d)
{
	return CommonFun::FromBinToString(d);
}
QString RobotHandKuka::ParseGo16(const QByteArray& d)
{
	return CommonFun::FromBinToString(d);
}
QString RobotHandKuka::ParseGi16Extend(const QByteArray& d)
{
	return CommonFun::FromBinToString(d);
}
QString RobotHandKuka::ParseGo16Extend(const QByteArray& d)
{
	return CommonFun::FromBinToString(d);
}
